import time

import serial
import serial.tools.list_ports
import threading

class SerialCommunication:
    def __init__(self):
        self.data = None#需要发送的数据
        self.serial_port = None #端口名称
        self.serial_thread = None
        self.connected = False #开关状态
        self.recieve_data = "" # 接受的数据
        self.cur_pos = 0 #位置
        self.cur_pos_lock = threading.Lock()  # 锁对象
        self.datalist: list = []  # 指令list
        self.will_steer_angel = 0.0

    def list_available_ports(self):
        available_ports = [port.device for port in serial.tools.list_ports.comports()]
        if len(available_ports)==0:
            available_ports.append("无")
        #print(available_ports)
        ports_txt=",".join(available_ports)
        print(ports_txt)
        #return available_ports
        return ports_txt

    #端口名，波特率，字节大小，校验位，停止位，
    def connect(self, port_name, baudrate, bytesize, parity, stopbits):
        try:
            self.serial_port = serial.Serial(port_name,
                                             baudrate=baudrate,
                                             bytesize=bytesize,
                                             parity=parity,
                                             stopbits=stopbits,
                                             timeout=0.01)#读超时时间
            self.connected = True
            #创建一个线程来不阻塞的读取数据
            self.serial_thread = threading.Thread(target=self.read_data)
            self.serial_thread.daemon = True
            self.serial_thread.start()
            self.write_data('S8\r')
            self.write_data('Y5\r')
            self.write_data('O\r\n')

            #开始启动steer线程
            self.steer_thread = threading.Thread(target=self.thread_steer)
            self.steer_thread.daemon = True
            self.steer_thread.start()

            return True
        except Exception as e:
            print(f"Failed to connect to {port_name}: {str(e)}")
            return False

    def thread_steer(self):
        while 1:
            self.steer_angel(self.will_steer_angel)
            time.sleep(0.02)


    #每次读取了数据后，更新当前位置
    def read_data(self):
        while self.connected:
            if self.serial_port.in_waiting > 0:
                data = self.serial_port.readline()
                #print(f"Received: {data}")
                self.recieve_data = data.decode('ASCII').strip() #str
                self.recieve_data = self.recieve_data[5:]
                self.recieve_pos = self.recieve_data[0:10]
                new_pos = self.rePOS_to_Pos(self.recieve_pos)
                # 使用锁更新 self.cur_pos
                with self.cur_pos_lock:
                    self.cur_pos = new_pos
                #print(f"当前位置: {self.cur_pos}")  # 调试输出当前位置

    def write_data(self, data):
        if self.connected:
            self.serial_port.write(data.encode('ASCII'))

    def disconnect(self):
        if self.connected:
            self.connected = False
            if self.serial_thread:
                self.serial_thread.join()
            if self.serial_port:
                self.serial_port.close()

    # 映射输入一角度，转换为Pos（十进制）
    def angle_to_Pos_mapping(self, angle: float) -> int:
        multiplier: float = 1048575 / 365
        Pos: int = int(angle * multiplier)
        return Pos

    def angel_to_list(self, angle: float) -> list:
        data_list = []
        Pos_dec = self.angle_to_Pos_mapping(angle)
        Pos_Hex = hex(Pos_dec)[2:]
        # 将Pos_Hex填充到5个数据位
        padded_hex = Pos_Hex.zfill(10)
        # 每两个字符一组，添加到data_list中
        for i in range(0, len(padded_hex), 2):
            data_list.append(padded_hex[i:i + 2])
        # print(data_list)
        return data_list

    def vec_to_list(self, vel) -> list:
        jiduizi = 14
        data_list = []
        vel_dec = int(vel * jiduizi / 60 / 1000 * 8388608)
        vel_Hex = hex(vel_dec)[2:]
        # 将vel_Hex填充到4个数据位
        padded_hex = vel_Hex.zfill(8)
        # 每两个字符一组，添加到data_list中
        for i in range(0, len(padded_hex), 2):
            data_list.append(padded_hex[i:i + 2])
        # print(data_list)
        return data_list

    def cur_to_list(self, cur) -> list:
        data_list = []
        cur_dec = int(cur * 32768 / 100)
        cur_Hex = hex(cur_dec)[2:]
        # 将cur_Hex填充到2个数据位
        padded_hex = cur_Hex.zfill(4)
        # 每两个字符一组，添加到data_list中
        for i in range(0, len(padded_hex), 2):
            data_list.append(padded_hex[i:i + 2])
        # print(data_list)
        return data_list

    # 控制旋转角度,CAN-FD协议,0-1048576，5Byte
    def steer_angel(self, angel: float, vel=10, cur=0.8):
        try:  # 整理8字节的位置信息
            self.datalist.clear()
            self.datalist = self.angel_to_list(angel) + \
                            self.vec_to_list(vel) + \
                            self.cur_to_list(cur) + \
                            ['02', '01', '00', '00', '00']  # +其他,02模式，01开，00kp，00kd

            output = "d001A"
            for i in range(len(self.datalist)):
                output = output + self.datalist[i]
            output = output + "\r"
            self.write_data(output)
            return True
        except:
            print(f"发送失败，欲发送{self.datalist}")
            return False

    # 接受POS数据转十进制角度365制
    def rePOS_to_Pos(self, recieve_pos) -> float:
        multiplier: float = 1048575 / 365
        dec_repos = int(recieve_pos, 16)
        pos = dec_repos % 1048576 / multiplier
        return pos

    # 接受Vel数据转10进制速度
    def reVEL_to_Vel(self, recieve_vel) -> float:
        jiduizi = 14
        dec_revel = int(recieve_vel, 16)
        vel = (dec_revel / 8388608) * 1000 * 60 / jiduizi
        return vel

    # 接受cur数据转10进制电流
    def reCur_to_Cur(self, recieve_cur) -> float:
        dec_recur = int(recieve_cur, 16)
        cur = dec_recur * 100 / 32768
        return cur


